package top.laoshuzi.cloudplatform.service;

import android.hardware.SensorManager;
import android.support.annotation.IntDef;

import com.google.android.things.pio.I2cDevice;
import com.google.android.things.pio.PeripheralManagerService;

import java.io.IOException;
import java.lang.annotation.Retention;
import java.lang.annotation.RetentionPolicy;

import top.laoshuzi.cloudplatform.tools.L;

/**
 * Created by mouse on 2017/12/21.
 */

public class Mpu6050 implements AutoCloseable {

    private static final String TAG = Mpu6050.class.getSimpleName();
    /**
     * I2C slave address of the MPU6050.
     */
    public static final int I2C_ADDRESS = 0x68;

    static final int REG_POWER_CFG1 = 0x6B;
    static final int REG_POWER_CFG2 = 0x6C;

    static final int REG_FIFO_ENABLE = 0x23;
    static final int REG_INT_ENABLE = 0x38;

    static final int REG_SAMPLING_RATE = 0x19;
    static final int REG_DLPF = 0x1A;

    static final int REG_ACC_CFG = 0x1C;
    static final int REG_GYRO_CFG = 0x1B;

    static final int REG_ACC_DATA = 0x3B;   // 0x3B ~ 0x40
    static final int REG_GYRO_DATA = 0x43;  // 0x43 ~ 0x48
    static final int REG_TEMP_DATA = 0x41;  // 0x41 ~ 0x42

    //
    static final int MAX_RATE_FREQ_HZ = 1000;
    static final int MIN_RATE_FREQ_HZ = 4;


    private I2cDevice mDevice;

    /**
     * Power mode.
     */
    @Retention(RetentionPolicy.SOURCE)
    @IntDef({POWE_MODE_STANDBY, POWE_MODE_ACTIVE})
    public @interface PowerMode {
    }

    public static final int POWE_MODE_ACTIVE = 0; // i2c on, output on
    public static final int POWE_MODE_STANDBY = 1; // i2c on, output off, low power

    /**
     * Accelerometer Range.
     */
    @Retention(RetentionPolicy.SOURCE)
    @IntDef({ACCELEROMETER_RANGE_AFS_2, ACCELEROMETER_RANGE_AFS_4, ACCELEROMETER_RANGE_AFS_8, ACCELEROMETER_RANGE_AFS_16})
    public @interface AccelerometerRange {
    }

    public static final int ACCELEROMETER_RANGE_AFS_2 = 0; // -2g ~ +2g
    public static final int ACCELEROMETER_RANGE_AFS_4 = 1; // -4g ~ +4g
    public static final int ACCELEROMETER_RANGE_AFS_8 = 2; // -8g ~ +8g
    public static final int ACCELEROMETER_RANGE_AFS_16 = 3; // -16g ~ +16g

    /**
     * Gyroscope Range.
     */
    @Retention(RetentionPolicy.SOURCE)
    @IntDef({GYROSCOPE_RANGE_FS_250, GYROSCOPE_RANGE_FS_500, GYROSCOPE_RANGE_FS_1000, GYROSCOPE_RANGE_FS_2000})
    public @interface GyroscopeRange {
    }

    public static final int GYROSCOPE_RANGE_FS_250 = 0; // -250°/s ~ +250°/s
    public static final int GYROSCOPE_RANGE_FS_500 = 1; // -500°/s ~ +500°/s
    public static final int GYROSCOPE_RANGE_FS_1000 = 2; // -1000°/s ~ +1000°/s
    public static final int GYROSCOPE_RANGE_FS_2000 = 3; // -2000°/s ~ +2000°/s


    public Mpu6050(String bus) throws IOException {
        PeripheralManagerService pioService = new PeripheralManagerService();
        I2cDevice device = pioService.openI2cDevice(bus, I2C_ADDRESS);
        try {
            connect(device);
        } catch (IOException | RuntimeException e) {
            try {
                close();
            } catch (IOException | RuntimeException ignored) {
                throw e;
            }
        }
    }

    private void connect(I2cDevice device) throws IOException {
        if (mDevice != null) {
            throw new IllegalStateException("device already connected");
        }
        mDevice = device;

        mDevice.writeRegByte(REG_FIFO_ENABLE, (byte) 0x0); // FIFO使能
        mDevice.writeRegByte(REG_INT_ENABLE, (byte) 0x0); // 中断使能
        mDevice.writeRegByte(REG_SAMPLING_RATE, (byte) 0x7); // 采样率分频
        mDevice.writeRegByte(REG_DLPF, (byte) 0x03); // 低通滤波配置

        setAccelerometerRange(ACCELEROMETER_RANGE_AFS_2);
        setGyroscopeRange(GYROSCOPE_RANGE_FS_2000);
    }

    @Override
    public void close() throws IOException {
        if (mDevice != null) {
            try {
                mDevice.close();
            } finally {
                mDevice = null;
            }
        }
    }

    public void reset() throws IOException {
        mDevice.writeRegByte(REG_POWER_CFG1, (byte) 0x80);
    }

    public void setPowerMode(@PowerMode int mode) throws IOException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }
        byte data = mDevice.readRegByte(REG_POWER_CFG1);
        if (mode == POWE_MODE_STANDBY) {
            data |= 1 << 6;
        } else if (mode == POWE_MODE_ACTIVE) {
            data &= ~1 << 6;
        }
        L.d("设置电源1配置寄存器:" + data);
        mDevice.writeRegByte(REG_POWER_CFG1, data);
    }

    @SuppressWarnings("ResourceType")
    public @PowerMode
    int getPowerMode() throws IOException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }
        byte data = mDevice.readRegByte(REG_POWER_CFG1);
        L.d("获取电源1配置寄存器:" + data);
        return data >> 6 & 0x1;
    }

    public void setAccelerometerRange(@AccelerometerRange int range) throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }
        byte data = mDevice.readRegByte(REG_ACC_CFG);
        data &= ~(0x3 << 3);
        data |= range << 3;
        L.d("设置加速计配置寄存器:" + data);
        mDevice.writeRegByte(REG_ACC_CFG, data);
    }

    @SuppressWarnings("ResourceType")
    public @AccelerometerRange
    int getAccelerometerRange() throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }
        byte data = mDevice.readRegByte(REG_ACC_CFG);
        L.d("获取加速计配置寄存器:" + data);
        return data >> 3 & 0x3;
    }

    public void setGyroscopeRange(@GyroscopeRange int range) throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }
        byte data = mDevice.readRegByte(REG_GYRO_CFG);
        data &= ~(0x3 << 3);
        data |= range << 3;
        L.d("设置陀螺仪配置寄存器:" + data);
        mDevice.writeRegByte(REG_GYRO_CFG, data);
    }

    @SuppressWarnings("ResourceType")
    public @GyroscopeRange
    int getGyroscopeRange() throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }
        byte data = mDevice.readRegByte(REG_GYRO_CFG);
        L.d("获取陀螺仪配置寄存器:" + data);
        return data >> 3 & 0x3;
    }

    public int getSamplingRate() throws IOException {
        return 1000 / (mDevice.readRegByte(REG_SAMPLING_RATE) + 1);
    }

    public float[] readSimple() throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }
        byte[] bytes = new byte[14];
        mDevice.readRegBuffer(REG_ACC_DATA, bytes, 14);
        float ax = (bytes[0] << 8 | bytes[1]) / 16384f * SensorManager.GRAVITY_EARTH;
        float ay = (bytes[2] << 8 | bytes[3]) / 16384f * SensorManager.GRAVITY_EARTH;
        float az = (bytes[4] << 8 | bytes[5]) / 16384f * SensorManager.GRAVITY_EARTH;
        float t = (bytes[6] << 8 | bytes[7]) / 340f + 36.25f;
        float gx = (bytes[8] << 8 | bytes[9]) / 16.384f;
        float gy = (bytes[10] << 8 | bytes[11]) / 16.384f;
        float gz = (bytes[12] << 8 | bytes[13]) / 16.384f;
        return new float[]{ax, ay, az, gx, gy, gz, t};
    }

    public float readTemperature() throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }

        byte[] sample = new byte[2];
        mDevice.readRegBuffer(REG_ACC_DATA, sample, 2);
        int temp = sample[0] << 8 | sample[1];
        float t = 36.25f + temp / 340f;
        return t;
    }

    public float[] readAcceleration() throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }

        byte[] sample = new byte[6];
        mDevice.readRegBuffer(REG_ACC_DATA, sample, 6);
        int ax = sample[0] << 8 | sample[1];
        int ay = sample[2] << 8 | sample[3];
        int az = sample[4] << 8 | sample[5];
        float x = ax / 16384f * SensorManager.GRAVITY_EARTH;
        float y = ay / 16384f * SensorManager.GRAVITY_EARTH;
        float z = az / 16384f * SensorManager.GRAVITY_EARTH;
        return new float[]{x, y, z};
    }

    public float[] readAngleSpeed() throws IOException, IllegalStateException {
        if (mDevice == null) {
            throw new IllegalStateException("device not connected");
        }

        byte[] sample = new byte[6];
        mDevice.readRegBuffer(REG_GYRO_DATA, sample, 6);
        int gx = sample[0] << 8 | sample[1];
        int gy = sample[2] << 8 | sample[3];
        int gz = sample[4] << 8 | sample[5];
        float x = gx / 16.384f;
        float y = gy / 16.384f;
        float z = gz / 16.384f;
        return new float[]{x, y, z};
    }


}
